#include "model/eyebrows_model.h"

void EyebrowsModel::init() {
	emotion_handler_sub_ = nh_.subscribe(Topic::EMO_HANDLER_BROW, 1, &EyebrowsModel::handlerCallback, this);

	joint_state_.name.resize(1);
	joint_state_.position.resize(1);

	joint_state_.name[0] = Joint::EYEBROWS;
}

void EyebrowsModel::spin() {
	ros::Rate loop_rate(30);
	setInPosition(true);

	while (ros::ok()) {
		if (!isInPosition()) {
			moveEyebrows();
		}

		prepState();
		joint_pub_.publish(joint_state_);
		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Handler Callback
 * Receives which position the Eyebrows have to be in.
 *
 * @param		state	Int32, 0 = NEUTRAL, 1 = UP, 2 = DOWN
 */
void EyebrowsModel::handlerCallback(const comm::eyebrows::ConstPtr &state) {
	int pos = state->state;

	if (position_ != pos) {
		position_ = pos;
		setInPosition(false);
	} else if (pos == EYEBROWS_NEUTRAL) {
		position_ = pos;
		angle_ = state->angle;
	}
}

void EyebrowsModel::moveEyebrows() {
	if (position_ == EYEBROWS_UP) {
		if (angle_ > ModelValues::EYEBROWS_MIN_ROTATION) {
			angle_ -= ModelValues::EYEBROWS_STEP;
		}
	} else if (position_ == EYEBROWS_DOWN) {
		if (angle_ < ModelValues::EYEBROWS_MAX_ROTATION) {
			angle_ += ModelValues::EYEBROWS_STEP;
		}
	} else {
		setInPosition(true);
	}
}

void EyebrowsModel::prepState() {
	joint_state_.header.stamp = ros::Time::now();
	joint_state_.position[0] = angle_;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "eyebrows_model_publisher");

	EyebrowsModel eyebrows;
	eyebrows.init();
	eyebrows.spin();
}
